#include <algorithm>
#include <cmath>
#include <cassert>

#include "Vector3.h"
#include "Matrix33.h"
#include "Matrix44.h"
#include "Quaternion.h"
#include "Mathutil.h"


using namespace gti;

matrix44::matrix44()
{
}


matrix44::matrix44(float a11,float a12,float a13,float a14,
				   float a21,float a22,float a23,float a24,
				   float a31,float a32,float a33,float a34,
				   float a41,float a42,float a43,float a44):
_11(a11),_12(a12),_13(a13),_14(a14),
_21(a21),_22(a22),_23(a23),_24(a24),
_31(a31),_32(a32),_33(a33),_34(a34),
_41(a41),_42(a42),_43(a43),_44(a44)
{
}

matrix44::matrix44(
		double a11,double a12,double a13,double a14,
		double a21,double a22,double a23,double a24,
		double a31,double a32,double a33,double a34,
		double a41,double a42,double a43,double a44):
_11(a11),_12(a12),_13(a13),_14(a14),
_21(a21),_22(a22),_23(a23),_24(a24),
_31(a31),_32(a32),_33(a33),_34(a34),
_41(a41),_42(a42),_43(a43),_44(a44)
{
}

matrix44::matrix44(double* data)
{
	for (int i = 0; i < 16; ++i)
		*(m + i) = *(data + i);
}


matrix44::matrix44(const matrix44& matrix) 
{ 
	set((float*)matrix.m); 
}

matrix44& matrix44::identity()
{
	m[0]=1; m[4]=0; m[8]=0; m[12]=0;
	m[1]=0; m[5]=1; m[9]=0; m[13]=0;
	m[2]=0; m[6]=0; m[10]=1; m[14]=0;
	m[3]=0; m[7]=0; m[11]=0; m[15]=1;
    return (*this);
}

matrix44& matrix44::transpose()
{
   std::swap(m[1],m[4]); std::swap(m[2],m[8]); std::swap(m[3],m[12]);
   std::swap(m[6],m[9]); std::swap(m[7],m[13]); std::swap(m[11],m[14]);
   return (*this);
}

void matrix44::set(const float *src)
{
	memcpy(m, src, 16*sizeof(float));
}

void matrix44::set(const double *src)
{
	for (int i = 0; i < 16; ++i)
		*(m + i) = *(src + i);
}

void matrix44::set(const ::quaternion& q, const vector3f& t)
{
	q.toMatrix(*this);
	_41 = t.x;_42 = t.y;_43 = t.z;_44 = 1.0f;
}

void matrix44::setInverse(const quaternion& q, const vector3f& t)
{
	const quaternion& iq = q.invert();
	set(iq,iq.rotate(-t));
}

void matrix44::set(const matrix33& mat, const vector3f& t)
{
	_11 = mat._11;_12 = mat._12;_13 = mat._13;
	_21 = mat._21;_22 = mat._22;_23 = mat._23;
	_31 = mat._31;_32 = mat._32;_33 = mat._33;
	_14 = _24 = _34 = 0.0f;
	_41 = t.x;_42 = t.y;_43 = t.z;_44 = 1.0f;
}

void matrix44::setBias()
{
	identity();
	_11 = 0.5;
	_22 = 0.5;
	_33 = 0.5;
	_41 = 0.5;
	_42 = 0.5;
	_43 = 0.5;
	_44 = 1;
}

void matrix44::clear()
{
	memset(m, 0, 16*sizeof(float));
}

matrix44 matrix44::operator+(const matrix44 &matrix) const
{
  matrix44 res;
	for (int i=0;i<16;i++) // TODO: unroll
	  res[i] = m[i]+matrix[i];
	return res;
}

void matrix44::operator+=(const matrix44 &matrix)
{
	for (int i=0;i<16;i++) // TODO: unroll
	  m[i] = m[i]+matrix[i];
}

matrix44 matrix44::operator-(const matrix44 &matrix) const
{
	matrix44 res;
	for (int i=0;i<16;i++) // TODO: unroll
	  res[i] = m[i]-matrix[i];
	return res;
}

matrix44 matrix44::operator*(const matrix44& matrix) const
{
  matrix44 ret;

	unsigned int i,j,k;
	for (i=0;i<4;i++) 	
	{
		for (j=0;j<4;j++) 
		{
			ret.M[i][j]=0.0;
			for (k=0;k<4;k++) 
				ret.M[i][j] += M[i][k] * matrix.M[k][j];
		}
	}

  return ret;
}

matrix44 gti::operator*(const matrix44& matrix, float f)
{
	matrix44 res;
	for (int i=0;i<16;i++) // TODO: unroll
	  res[i] = matrix[i]*f;
	return res;
}

matrix44 gti::operator*(float f,const matrix44& matrix)
{
	matrix44 res;
	for (int i=0;i<16;i++) // TODO: unroll
	  res[i] = matrix[i]*f;
	return res;
}

void matrix44::operator*=(const matrix44& matrix)
{
// TO DO: avoid matrix copy
  matrix44 res = *this*matrix;
  *(this) = res;
}

void matrix44::operator*=(float f)
{
	for (int i=0;i<16;i++) // TODO: unroll
	  m[i]*= f;

}

vector3f gti::operator*(const matrix44& matrix, const vector3f& v)
{   
   float x = matrix.m[0] * v.x + matrix.m[4] * v.y + matrix.m[8] * v.z + matrix.m[12]; 
   float y = matrix.m[1] * v.x + matrix.m[5] * v.y + matrix.m[9] * v.z + matrix.m[13]; 
   float z = matrix.m[2] * v.x + matrix.m[6] * v.y + matrix.m[10] * v.z + matrix.m[14];
	 //float w = matrix.m[3] * v.x + matrix.m[7] * v.y + matrix.m[11] * v.z + matrix.m[15];
	 //float inv=1/w;
   //return vector3f(x*inv,y*inv,z*inv);
   return vector3f(x,y,z);
}


bool gti::operator == (const matrix44& m1, const matrix44& m2)
{
	return (!memcmp(m1.m,m2.m,16));
}

bool gti::operator != (const matrix44& m1, const matrix44& m2)
{
	return !(m1 == m2);
}


float matrix44::trace() const
{
	return (_11 + _22 + _33 + _44);
}

void matrix44::getQuaternion(quaternion &q) const
{
	float trace = _11 + _22 + _33;

    if ( trace > 0 )
    {
        // |w| > 1/2, may as well choose w > 1/2
        float fRoot = sqrt(trace + 1.0f);  // 2w
        q.w = 0.5f*fRoot;
        fRoot = 0.5f/fRoot;  // 1/(4w)
        q.x = (_32-_23)*fRoot;
        q.y = (_13-_31)*fRoot;
        q.z = (_21-_12)*fRoot;
    }
    else
    {
        // |w| <= 1/2
        int i = 0;
        if ( _22 > _11 )
            i = 1;
        if ( _33 > M[i][i] )
            i = 2;
        int j = (i+1) % 3;
        int k = (j+1) % 3;

        float fRoot = sqrtf( M[i][i]- M[j][j]- M[k][k]+1.0f);
        float* apfQuat[3] = { &q[0], &q[1], &q[2] };
        *apfQuat[i] = 0.5f*fRoot;
        fRoot = 0.5f/fRoot;
        q.w = (M[k][j]-M[j][k])*fRoot;
        *apfQuat[j] = (M[j][i]+M[i][j])*fRoot;
        *apfQuat[k] = (M[k][i]+M[i][k])*fRoot;
    }
}

matrix44 operator+(const matrix44& matrix,float f)
{
	matrix44 res;
	for (int i=0;i<16;i++) // TODO: unroll
	  res[i] = matrix[i]+f;
	return res;
}

matrix44 operator+(float f,const matrix44& matrix)
{
  matrix44 res;
	for (int i=0;i<16;i++) // TODO: unroll
	  res[i] = matrix[i]+f;
	return res;
}

matrix44 operator-(const matrix44& matrix, float f)
{
  matrix44 res;
	for (int i=0;i<16;i++) // TODO: unroll
	  res[i] = matrix[i]-f;
	return res;
}

matrix44 operator-(float f,const matrix44& matrix)
{
	matrix44 res;
	for (int i=0;i<16;i++) // TODO: unroll
	  res[i] = matrix[i]-f;
	return res;
}


bool matrix44::inverse(matrix44& inverse) const
{
   unsigned int i, j, k, swap;
   float t;
   matrix44 temp;
   inverse.identity();

   temp = (*this);

   unsigned int m,n;
   m = n = 4;
	
   for (i = 0; i < m; i++)
   {
      // Look for largest element in column

      swap = i;
      for (j = i + 1; j < m; j++)// m or n
	  {
//         if ( fabs(temp(j,i)) > fabs(temp(i,i)) )
		 if ( fabs(temp(j,i)) > fabs(temp(swap,i)) )
            swap = j;
	  }
   
      if (swap != i)
      {
         // Swap rows.

         for (k = 0; k < n; k++)
         {
			 std::swap(temp(i,k),temp(swap,k));
			 std::swap(inverse(i,k),inverse(swap,k));
         }
      }

      // No non-zero pivot.  The CMatrix is singular, which shouldn't
      // happen.  This means the user gave us a bad CMatrix.


#define MATRIX_SINGULAR_THRESHOLD 0.0001

      if ( fabsf(temp(i,i)) <= MATRIX_SINGULAR_THRESHOLD)
	  {
		  inverse.identity();
         return false;
	  }
#undef MATRIX_SINGULAR_THRESHOLD

      t = 1.0f/temp(i,i);

      for (k = 0; k < n; k++)//m or n
      {
         temp(i,k) *= t;
         inverse(i,k) *= t;
      }

      for (j = 0; j < m; j++) // m or n
      {
         if (j != i)
         {
            t = temp(j,i);
            for (k = 0; k < n; k++)//m or n
            {
               temp(j,k) -= (temp(i,k) * t);
               inverse(j,k) -= (inverse(i,k) * t);
            }
         }
      }
   }

   return true;
}

void matrix44::project(const vector3f& src, vector3f& dst) const
{
   dst.x = m[0] * src.x + m[4] * src.y + m[8] * src.z + m[12]; 
   dst.y = m[1] * src.x + m[5] * src.y + m[9] * src.z + m[13]; 
   dst.z = m[2] * src.x + m[6] * src.y + m[10] * src.z + m[14];
   float w = m[3] * src.x + m[7] * src.y + m[11] * src.z + m[15];
   float inv = 1.0f/w;
   dst.x *= inv;
   dst.y *= inv;
   dst.z *= inv;
}

void matrix44::setNoTranslation()
{
	_41 = _42 = _43 = 0;
}

void matrix44::setRow(int row, const vector3f& axis)
{
	m[row] = axis.x;
	m[row+4] = axis.y;
	m[row+8] = axis.z;
}

void matrix44::setColumn(int col, const vector3f& axis)
{
	int idx = col*4;
	m[idx] = axis.x;
	m[idx+1] = axis.y;
	m[idx+2] = axis.z;
}

void matrix44::setTranslation(const vector3f& trans)
{
	setColumn(3,trans);
}

void matrix44::setTranslationMatrix(const vector3f& trans)
{
	identity();
	setColumn(3,trans);
}

void matrix44::setScale(const vector3f& scale)
{
	_11 = scale.x;
	_22 = scale.y;
	_33 = scale.z;
}

void matrix44::setScaleMatrix(const vector3f& scale)
{
	identity();
	_11 = scale.x;
	_22 = scale.y;
	_33 = scale.z;
}

void matrix44::setRotation(float angle, vector3f axis)
{
	axis.normalize();
	quaternion q(axis,angle);
	matrix44 m;
	q.toMatrix(m);
	_11 = m._11; _12 = m._12; _13 = m._13;
	_21 = m._21; _22 = m._22; _23 = m._23;
	_31 = m._31; _32 = m._32; _33 = m._33;
}

void matrix44::setOrthoProjection(float ortho_left, float ortho_right, float ortho_bottom, float ortho_top, float z_near, float z_far)
{
// glOrtho call
//
// |     2/(r-l)       0            0       (r+l)/(r-l)  |
// |      0           2/(t-b)       0       (t+b)/(t-b)  |
// |      0            0         -2/(f-n)   (f+n)/(f-n)  |
// |      0            0            0            1       |
// 
	const float width = ortho_right - ortho_left;
	const float height = ortho_top - ortho_bottom;
	const float depth = z_far - z_near;
	vector3f translation;
	
	clear();
	_11 = 2.0f / width;
	_22 = 2.0f / height;
	_33 = -2.0f / depth;

	translation.set(
		-(ortho_right + ortho_left)/width,
		-(ortho_top + ortho_bottom)/height,
		-(z_far + z_near)/depth
		);
	setTranslation(translation);
	_44 = 1;
}

void matrix44::setProjection(float fov, float aspect, float near, float far)
{
// glFrustum call (column order)
//
// |    2n/(r-l)       0         (r+l)/(r-l)     0       |
// |      0          2n/(t-b)    (t+b)/(t-b)     0       |
// |      0            0         (n+f)/(n-f) (2nf)/(n-f) |
// |      0            0              -1         0       |
//
// gluPerspective call (column order)
// f = cotg (fov/2)
//
// |    f/aspect		0			0			0			|
// |      0				f			0			0			|
// |      0				0	   (n+f)/(n-f)	(2nf)/(n-f)     |
// |      0				0			-1			0			|
// 
	float cotg = 1.0f/tanf( deg2rad(fov) * 0.5f );
	float zs = near - far;
	if (fabsf(zs) < 0.00001f)
	{
		assert(0 && "compute projection matrix, division by zero"); //division by zero
		clear();
	}

	zs = 1.0f/zs;
	clear();
	_11 = cotg / aspect;
	_22 = cotg;
	_33 = (far+near)*zs;
	_43 = (2*far*near)*zs;
	_34 = -1;
}

void matrix44::copyRow(int id, vector3f& row) const
{
	row.x = m[id];
	row.y = m[id+4];
	row.z = m[id+8];
}

void matrix44::copyColumn(int id, vector3f& column) const
{
	int idx = id*4;
	column.x = m[idx];
	column.y = m[idx+1];
	column.z = m[idx+2];
}

void matrix44::copyTranslation(vector3f& trans) const
{
	copyColumn(3,trans);
}

vector3f matrix44::getTranslation() const
{
	return vector3f(_41,_42,_43);
}

vector3f matrix44::getScale() //WARNING: ERROR
{
	return vector3f(_11,_22,_33);
}

void matrix44::rotate(const vector3f& v, vector3f& rv) const
{
	assert(&v != &rv && "Impossible to use the same containers as target and source");
   rv.x = m[0] * v.x + m[4] * v.y + m[8 ] * v.z;// + matrix.m[12]; 
   rv.y = m[1] * v.x + m[5] * v.y + m[9 ] * v.z;// + matrix.m[13]; 
   rv.z = m[2] * v.x + m[6] * v.y + m[10] * v.z;// + matrix.m[14];
}

vector3f matrix44::rotateTransposed(const vector3f& v) const
{
  return vector3f(
		m[0] * v.x + m[1] * v.y + m[2 ] * v.z,// + matrix.m[12]; 
		m[4] * v.x + m[5] * v.y + m[6 ] * v.z,// + matrix.m[13]; 
		m[8] * v.x + m[9] * v.y + m[10] * v.z// + matrix.m[14];
		);
}

matrix33 matrix44::getRotationMatrix() const
{
	matrix33 m;
	m._11 = _11;m._12 = _12;m._13 = _13;
	m._21 = _21;m._22 = _22;m._23 = _23;
	m._31 = _31;m._32 = _32;m._33 = _33;
	return m;
}

vector4f matrix44::operator*(const vector4f& v) const
{
	vector4f v4;
	v4.x = m[0] * v.x + m[4] * v.y + m[8] * v.z + m[12] * v.w; 
	v4.y = m[1] * v.x + m[5] * v.y + m[9] * v.z + m[13] * v.w; 
	v4.z = m[2] * v.x + m[6] * v.y + m[10] * v.z + m[14] * v.w;
	v4.w = m[3] * v.x + m[7] * v.y + m[11] * v.z + m[15] * v.w;
	return v4;
}


float matrix44::getDistanceError( const matrix44 &b ) const {
	matrix44 c = (*this) - b;
	float err = 0;
	for( int i=0; i<16; ++i ) {
		err += fabsf( c.m[ i ] );
	}
	return err;
}

// up vector can be modified in order to generate a valid orthonormal matrix
void matrix44::cameraLookAt(const vector3f& eye, const vector3f& center, vector3f& up)
{
// gluLookAt call (column order) and
// D3DXMatrixLookAtRH() (column order) generate the same matrix
// zaxis = normal(Eye - At)
// xaxis = normal(cross(Up, zaxis))
// yaxis = cross(zaxis, xaxis)

//  xaxis.x           yaxis.x           zaxis.x          0
//  xaxis.y           yaxis.y           zaxis.y          0
//  xaxis.z           yaxis.z           zaxis.z          0
// -dot(xaxis, eye)  -dot(yaxis, eye)  -dot(zaxis, eye)  1

// D3DXMatrixLookAtLH() (column order) generate a D3DXMatrixLookAtRH
// but zaxis = normal(At - Eye)

	vector3f xaxis,yaxis,zaxis;
	zaxis = eye - center;
	zaxis.normalize();
	float dotp = DotProduct(up,zaxis);
	// angle between them is 180 
	if (fabsf(dotp) > 0.9999f) up.set(1,0,0);
	xaxis = CrossProduct(up, zaxis); xaxis.normalize();
	yaxis = CrossProduct(zaxis, xaxis);
//	up_vector = yaxis;//<<added now
		
	_11 = xaxis.x; _12 = yaxis.x; _13 = zaxis.x; _14 = 0;
	_21 = xaxis.y; _22 = yaxis.y; _23 = zaxis.y; _24 = 0;
	_31 = xaxis.z; _32 = yaxis.z; _33 = zaxis.z; _34 = 0;
	_41 = -DotProduct(xaxis, eye);
	_42 = -DotProduct(yaxis, eye);
	_43 = -DotProduct(zaxis, eye);
	_44 =  1;

}

// Multiplies the point by the inverse of the current matrix
// Note: the matrix must be a orthonormal matrix without scaling
gti::vector3f matrix44::multInverse(const gti::vector3f& point) const
{
	gti::vector3f p;
	// 1. negate translation
	p.x = point.x - m[12];
	p.y = point.y - m[13];
	p.z = point.z - m[14];

	// 2. negate rotation
	return rotateTransposed(p);
}
